/*
 * L298N.h
 * 
 */
#ifndef L298N_H
#define L298N_H

#include <stdio.h>
#include <wiringPi.h>
#include <softPwm.h>


#define PIN_LED_forward 7
#define PIN_LED_backward 8
#define PIN_LED_stop 9

#define Motor1_PWM 12
#define Motor1_IN0 13
#define Motor1_IN1 14 

#define Motor2_PWM 24
#define Motor2_IN0 22
#define Motor2_IN1 23 

#define Motor3_PWM 15
#define Motor3_IN0 16
#define Motor3_IN1 1 

#define Motor4_PWM 6
#define Motor4_IN0 10
#define Motor4_IN1 11 

int speed_val = 200; //0~1024
int speed_turn = 200;

void Motor_Setup(void)
{
	pinMode(PIN_LED_forward,OUTPUT);
	pinMode(PIN_LED_backward,OUTPUT);
	pinMode(PIN_LED_stop,OUTPUT);
	digitalWrite(PIN_LED_forward,0);
	digitalWrite(PIN_LED_backward,0);
	digitalWrite(PIN_LED_stop,0);
	
	pinMode(Motor1_IN0,OUTPUT);
	pinMode(Motor1_IN1,OUTPUT);
	digitalWrite(Motor1_IN0,0);
	digitalWrite(Motor1_IN1,0);
	
	pinMode(Motor2_IN0,OUTPUT);
	pinMode(Motor2_IN1,OUTPUT);
	digitalWrite(Motor2_IN0,0);
	digitalWrite(Motor3_IN1,0);
	
	pinMode(Motor3_IN0,OUTPUT);
	pinMode(Motor3_IN1,OUTPUT);
	digitalWrite(Motor3_IN0,0);
	digitalWrite(Motor3_IN1,0);
	
	pinMode(Motor4_IN0,OUTPUT);
	pinMode(Motor4_IN1,OUTPUT);
	digitalWrite(Motor4_IN0,0);
	digitalWrite(Motor4_IN1,0);
	
	softPwmCreate(Motor1_PWM, speed_val, 1000);
	softPwmCreate(Motor2_PWM, speed_val, 1000);
	softPwmCreate(Motor3_PWM, speed_val, 1000);
	softPwmCreate(Motor4_PWM, speed_val, 1000);
}

void Motor_Left(void)
{
	softPwmWrite (Motor1_PWM, 0);
	softPwmWrite (Motor2_PWM, speed_turn);
	softPwmWrite (Motor3_PWM, speed_turn);
	softPwmWrite (Motor4_PWM, 0);
	
	
	printf("左转...\n");
}

void Motor_Right(void)
{
	softPwmWrite (Motor1_PWM, speed_turn);
	softPwmWrite (Motor2_PWM, 0);
	softPwmWrite (Motor3_PWM, 0);
	softPwmWrite (Motor4_PWM, speed_turn);
	
	
	printf("右转...\n");
}

void Motor_Forward(void)
{
	softPwmWrite (Motor1_PWM, speed_val);
	softPwmWrite (Motor2_PWM, speed_val);
	softPwmWrite (Motor3_PWM, speed_val);
	softPwmWrite (Motor4_PWM, speed_val);
	
	digitalWrite(PIN_LED_forward,1);
	digitalWrite(PIN_LED_backward,0);
	digitalWrite(PIN_LED_stop,0);
	
	digitalWrite(Motor1_IN0,1);
	digitalWrite(Motor1_IN1,0);
	
	digitalWrite(Motor2_IN0,1);
	digitalWrite(Motor2_IN1,0);
	
	digitalWrite(Motor3_IN0,1);
	digitalWrite(Motor3_IN1,0);
	
	digitalWrite(Motor4_IN0,1);
	digitalWrite(Motor4_IN1,0);
	
	printf("正转...\n");
}

void Motor_Backward(void)
{
	softPwmWrite (Motor1_PWM, speed_val);
	softPwmWrite (Motor2_PWM, speed_val);
	softPwmWrite (Motor3_PWM, speed_val);
	softPwmWrite (Motor4_PWM, speed_val);
	
	digitalWrite(PIN_LED_forward,0);
	digitalWrite(PIN_LED_backward,1);
	digitalWrite(PIN_LED_stop,0);
	
	digitalWrite(Motor1_IN0,0);
	digitalWrite(Motor1_IN1,1);
	
	digitalWrite(Motor2_IN0,0);
	digitalWrite(Motor2_IN1,1);
	
	digitalWrite(Motor3_IN0,0);
	digitalWrite(Motor3_IN1,1);
	
	digitalWrite(Motor4_IN0,0);
	digitalWrite(Motor4_IN1,1);
	
	printf("反转...\n");
}

void Motor_Stop(void)
{
	softPwmWrite (Motor1_PWM, 0);
	softPwmWrite (Motor2_PWM, 0);
	softPwmWrite (Motor3_PWM, 0);
	softPwmWrite (Motor4_PWM, 0);
	
	digitalWrite(PIN_LED_forward,0);
	digitalWrite(PIN_LED_backward,0);
	digitalWrite(PIN_LED_stop,1);
	
	digitalWrite(Motor1_IN0,0);
	digitalWrite(Motor1_IN1,0);
	
	digitalWrite(Motor2_IN0,0);
	digitalWrite(Motor2_IN1,0);
	
	digitalWrite(Motor3_IN0,0);
	digitalWrite(Motor3_IN1,0);
	
	digitalWrite(Motor4_IN0,0);
	digitalWrite(Motor4_IN1,0);
	
	printf("停止...\n");
}

void speedUp(void)
{
	speed_val += 100;
	if(speed_val>=1024) speed_val=1000;
	softPwmWrite (Motor1_PWM, speed_val);
	softPwmWrite (Motor2_PWM, speed_val);
	softPwmWrite (Motor3_PWM, speed_val);
	softPwmWrite (Motor4_PWM, speed_val);
	printf("加速: %d %%\n",speed_val);
}

void speedDown(void)
{
	speed_val -= 100;
	if(speed_val<=0) speed_val=0;
	softPwmWrite (Motor1_PWM, speed_val);
	softPwmWrite (Motor2_PWM, speed_val);
	softPwmWrite (Motor3_PWM, speed_val);
	softPwmWrite (Motor4_PWM, speed_val);
	printf("减速: %d %%\n",speed_val);
}

void GPIO_Cleanup(void)
{
	Motor_Stop();
	digitalWrite(PIN_LED_stop,0);
	
	//pinMode(PIN_LED_forward,INPUT);
	//pinMode(PIN_LED_backward,INPUT);
	//pinMode(PIN_LED_stop,INPUT);
	
	pinMode(Motor1_PWM,INPUT);
	pinMode(Motor2_PWM,INPUT);
	pinMode(Motor3_PWM,INPUT);
	pinMode(Motor4_PWM,INPUT);
	
	pinMode(Motor1_IN0,INPUT);
	pinMode(Motor1_IN1,INPUT);
	
	pinMode(Motor2_IN0,INPUT);
	pinMode(Motor2_IN1,INPUT);
	
	pinMode(Motor3_IN0,INPUT);
	pinMode(Motor3_IN1,INPUT);
	
	pinMode(Motor4_IN0,INPUT);
	pinMode(Motor4_IN1,INPUT);

}
#endif
